#include "map.h"

/**************************************************************************/
/**************************************************************************/
/*                            Map Code                                    */
/**************************************************************************/
/**************************************************************************/
Map::Map()
{

}

void Map::start(int front, int nDist, int sDist, int eDist, int wDist)
{
    home = new Node(nDist, sDist, eDist, wDist, 0, 0);
    currentNode = home;
    inFront = front;
    currentDir = 'N';
    std::cout << "Map started" << std::endl;
    nodeList.append(home);
}

bool Map::addNode(int dist, int f, int b, int r, int l)
{
    int *coord;
    Node *next, *check;
    dist = inFront - dist;

    switch(currentDir)
    {
    case 'N':
        coord = currentNode->getCoord();
        //coord[1] += dist;
        check = same(coord[0], coord[1] + dist);
        if(check == NULL)
        {
            next = new Node(f, b, r, l, coord[0], coord[1] + dist);
            next->setS(currentNode, dist);
            next->setVisited(dirtoi('S'));
            currentNode->setN(next, dist);
            nodeList.append(next);
        }
        else
        {
            next = check;
        }
        break;
    case 'S':
        coord = currentNode->getCoord();
        //coord[1] -= dist;
        check = same(coord[0], coord[1] - dist);
        if(check == NULL)
        {
            next = new Node(b, f, l, r, coord[0], coord[1] - dist);
            next->setN(currentNode, dist);
            next->setVisited(dirtoi('N'));
            currentNode->setS(next, dist);
            nodeList.append(next);
        }
        else
        {
            next = check;
        }
        break;
    case 'E':
        coord = currentNode->getCoord();
        //coord[0] += dist;
        check = same(coord[0] + dist, coord[1]);
        if(check == NULL)
        {
            next = new Node(l, r, f, b, coord[0] + dist, coord[1]);
            next->setW(currentNode, dist);
            next->setVisited(dirtoi('W'));
            currentNode->setE(next, dist);
            nodeList.append(next);
        }
        else
        {
            next = check;
        }
        break;
    case 'W':
        coord = currentNode->getCoord();
        //coord[0] -= dist;
        check = same(coord[0] - dist, coord[1]);
        if(check == NULL)
        {
            next = new Node(r, l, b, f, coord[0] - dist, coord[1]);
            next->setE(currentNode, dist);
            next->setVisited(dirtoi('E'));
            currentNode->setW(next, dist);
            nodeList.append(next);
        }
        else
        {
            next = check;
        }
        break;
    }

    currentNode = next;

    std::cout << "Node added" << std::endl;

    return true;
}

char Map::oppDir(char dir)
{
    switch(dir)
    {
    case 'N':
        return 'S';
    case 'S':
        return 'N';
    case 'E':
        return 'W';
    case 'W':
        return 'E';
    default:
        return 'X';
    }
}

void Map::turn(char dir, int dist)
{
    inFront = dist;
    currentDir = changeDir(dir);
    currentNode->setVisited(dirtoi(currentDir));

    std::cout << "Current Dir " << currentDir << std::endl;
}

char Map::changeDir(char dir)
{
    if(dir == 'R')
    {
        switch(currentDir)
        {
        case 'N':
            return 'E';
        case 'S':
            return 'W';
        case 'E':
            return 'S';
        case 'W':
            return 'N';
        default:
            return 'X';
        }
    }
    else
    {
        switch(currentDir)
        {
        case 'N':
            return 'W';
        case 'S':
            return 'E';
        case 'E':
            return 'N';
        case 'W':
            return 'S';
        default:
            return 'X';
        }
    }
}

void Map::setRoom(char dir)
{
    currentNode->setVisited(dirtoi(changeDir(dir)));
    currentNode->setRoom(true);
}

void Map::inferConnections(int n, int s, int e, int w)
{
    Node *toCheck;
    int *curr, *check, *currDist, *checkDist, distAvg;

    curr = currentNode->getCoord();
    currDist = currentNode->getDist();

    for(int i = 0; i < nodeList.count(); i++)
    {
        toCheck = nodeList[i];
        check = toCheck->getCoord();
        checkDist = toCheck->getDist();
        for(int j = 0; j < 4; j++)
        {
            //check if there x values are similar
            if(similar(curr[0], check[0]))
            {
                //if current node is bellow check node
                if(curr[1] < check[1])
                {
                    //if current north dist similar to check south dist
                    if(similar(currDist[0], checkDist[1]) || currDist[0] > checkDist[1])
                    {
                        //make sure nodes do not already point to somewhere
                        if(currentNode->getN() == NULL && currentNode->getS() == NULL)
                        {
                            distAvg = (currDist[0] + checkDist[1]) / 2;
                            currentNode->setN(toCheck, distAvg);
                            toCheck->setS(currentNode, distAvg);
                        }
                    }
                }
                //if current node is above check node
                else if(curr[1] > check[1])
                {
                    //if current south dist similar to check north dist
                    if(similar(currDist[1], checkDist[0]) || currDist[1] > checkDist[0])
                    {
                        //make sure nodes do not already point to somewhere
                        if(currentNode->getS() == NULL && toCheck->getN() == NULL)
                        {
                            distAvg = (currDist[1] + checkDist[0]) / 2;
                            currentNode->setS(toCheck, distAvg);
                            toCheck->setN(currentNode, distAvg);
                        }
                    }
                }
            }
            //check if there y values are similar
            else if(similar(curr[1], check[1]))
            {
                //if curr is left of check
                if(curr[0] < check[0])
                {
                    //if curr right dist similar to check left dist
                    if(similar(currDist[2], checkDist[3]) || currDist[2] > checkDist[3])
                    {
                        //make sure nodes do not already point to somewhere
                        if(currentNode->getE() == NULL && toCheck->getW() == NULL)
                        {
                            distAvg = (currDist[2] + checkDist[3]) / 2;
                            currentNode->setE(toCheck, distAvg);
                            toCheck->setW(currentNode, distAvg);
                        }
                    }
                }
                //if curr is right of check
                else if(curr[0] > check[0])
                {
                    //if curr left dist is similar to check right dist
                    if(similar(currDist[3], currDist[2]) || currDist[3] > checkDist[2])
                    {
                        //make sure nodes do not already point to somewhere
                        if(currentNode->getW() == NULL && toCheck->getE() == NULL)
                        {
                            distAvg = (currDist[3] + checkDist[2]) / 2;
                            currentNode->setW(toCheck, distAvg);
                            toCheck->setE(currentNode, distAvg);
                        }
                    }
                }
            }
        }
    }
}

Node* Map::same(int x, int y)
{
    Node *toCheck;

    for(int i = 0; i < nodeList.count(); i++)
    {
        toCheck = nodeList[i];
        if(similar(x, toCheck->getCoord()[0]) && similar(y, toCheck->getCoord()[1]))
        {
            return toCheck;
        }
    }

    return NULL;
}

bool Map::similar(int val1, int val2)
{
    if(val1 < val2 + 10 && val1 > val2 - 10)
    {
        return true;
    }

    return false;
}

void Map::shortestPath()
{
    QQueue<Node*> nodes;

    Node* curr = currentNode;
    curr->setTravelDist(0);
    nodes.enqueue(curr);

    while(!nodes.empty())
    {
        curr = nodes.dequeue();
        for(int i = 0; i < 4; i++)
        {
            if(curr->getPtr(i) != NULL && i != dirtoi(curr->getPrev()))
            {
                if(curr->getPtr(i)->getTravelDist() == -1)
                {
                    curr->getPtr(i)->setPrev(oppDir(itodir(i)));
                    curr->getPtr(i)->setTravelDist(curr->getDist()[i] + curr->getTravelDist());
                    nodes.enqueue(curr->getPtr(i));
                }
                //         current nodes travel distance     dist from curr to next   dist traveled to curr
                else if(curr->getPtr(i)->getTravelDist() > curr->getDist()[i] + curr->getTravelDist())
                {
                    curr->getPtr(i)->setPrev(oppDir(itodir(i)));
                    curr->getPtr(i)->setTravelDist(curr->getDist()[i] + curr->getTravelDist());
                    nodes.enqueue(curr->getPtr(i));
                }
            }
        }
    }

    bool done = false;

    travel = new travelNode;
    travel->node = home;
    travel->isHome = true;
    travel->travelDir = 'X';
    travelList.push(travel);
    home->getPtr(dirtoi(home->getPrev()))->setNext(oppDir(home->getPrev()));
    curr = home->getPtr(dirtoi(home->getPrev()));
    while(done == false)
    {
        if(curr == currentNode)
        {
            done = true;
        }
        travel = new travelNode;
        travel->node = curr;
        travel->travelDir = curr->getNext();
        travelList.push(travel);

        if(curr == currentNode)
        {
            done = true;
        }
        else
        {
            curr->getPtr(dirtoi(curr->getPrev()))->setNext(oppDir(curr->getPrev()));
            curr = curr->getPtr(dirtoi(curr->getPrev()));
        }

    }
}

char Map::whereNext()
{
    travel = travelList.pop();

    if(travel->isHome == true)
    {
        return 'H';
    }

    return turnDir(travel->travelDir);
}

int Map::dirtoi(char dir)
{
    switch(dir)
    {
    case 'N':
        return 0;
    case 'S':
        return 1;
    case 'E':
        return 2;
    case 'W':
        return 3;
    default:
        return -1;
    }
}

char Map::itodir(int dir)
{
    switch(dir)
    {
    case 0:
        return 'N';
    case 1:
        return 'S';
    case 2:
        return 'E';
    case 3:
        return 'W';
    default:
        return 'X';
    }
}

char Map::checkNext()
{
    int unVisitedAmt[4];
    int unVisited = 0;
    for(int i = 0; i < 4; i++)
    {
        unVisitedAmt[i] = 0;
    }

    for(int i = 0; i < 4; i++)
    {
        if(currentNode->getDist()[i] > 20 && currentNode->getVisited()[i] == false)
        {
            return itodir(i);
        }
    }

    for(int i = 0; i < 4; i++)
    {
        if(currentNode->getDist()[i] > 20 && currentNode->getPtr(i) != NULL)
        {
            unVisitedAmt[i] = recurseNext(currentNode->getPtr(i));
        }
    }

    for(int i = 0; i < 4; i++)
    {
        if(unVisitedAmt[i] > unVisitedAmt[unVisited])
        {
            unVisited = i;
        }
    }

    return turnDir(itodir(unVisited));
}

int Map::recurseNext(Node *next)
{
    int unVisitedCount = 0;

    for(int i = 0; i < 4; i++)
    {
        if(next->getDist()[i] > 20 && next->getVisited()[i] == false)
        {
            unVisitedCount++;
        }
    }

    for(int i = 0; i < 4; i++)
    {
        if(next->getDist()[i] > 20 && next->getPtr(i) != NULL)
        {
            unVisitedCount += recurseNext(next->getPtr(i));
        }
    }

    return unVisitedCount;
}

char Map::turnDir(char dir)
{
    switch(currentDir)
    {
    case 'N':
        switch(dir)
        {
        case 'N':
            return 'F';
        case 'S':
            return 'B';
        case 'E':
            return 'R';
        case 'W':
            return 'L';
        }
    case 'S':
        switch(dir)
        {
        case 'N':
            return 'B';
        case 'S':
            return 'F';
        case 'E':
            return 'L';
        case 'W':
            return 'R';
        }
    case 'E':
        switch(dir)
        {
        case 'N':
            return 'L';
        case 'S':
            return 'R';
        case 'E':
            return 'F';
        case 'W':
            return 'B';
        }
    case 'W':
        switch(dir)
        {
        case 'N':
            return 'R';
        case 'S':
            return 'L';
        case 'E':
            return 'B';
        case 'W':
            return 'F';
        }
    default:
        return 'X';
    }
}

/**************************************************************************/
/**************************************************************************/
/*                           Node Code                                    */
/**************************************************************************/
/**************************************************************************/


/**************************************************************************/
/*                           Constructors                                 */
/**************************************************************************/

Node::Node()
{
    for(int i = 0; i < 4; i++)
    {
        nodePts[i] = NULL;
        nodeDist[i] = -1;
        visited[i] = false;
    }
    setCoord(0, 0);
    next = NULL;
    prev = NULL;
    travelDist = -1;
}

Node::Node(int nDist, int sDist, int eDist, int wDist, int x, int y)
{
    nodeDist[0] = nDist;
    nodeDist[1] = sDist;
    nodeDist[2] = eDist;
    nodeDist[3] = wDist;
    for(int i = 0; i < 4; i++)
    {
        nodePts[i] = NULL;
        visited[i] = false;
    }
    setCoord(x, y);
    next = NULL;
    prev = NULL;
    travelDist = -1;
}


/**************************************************************************/
/*                           Sets                                         */
/**************************************************************************/


void Node::setN(Node *N, int dist)
{
    nodePts[0] = N;
    nodeDist[0] = dist;
}

void Node::setS(Node *S, int dist)
{
    nodePts[1] = S;
    nodeDist[1] = dist;
}

void Node::setE(Node *E, int dist)
{
    nodePts[2] = E;
    nodeDist[2] = dist;
}

void Node::setW(Node *W, int dist)
{
    nodePts[3] = W;
    nodeDist[3] = dist;
}

void Node::setTravelDist(int dist)
{
    travelDist = dist;
}

void Node::setPrev(char prevNode)
{
    prev = prevNode;
}

void Node::setNext(char nextNode)
{
    next = nextNode;
}

void Node::setCoord(int x, int y)
{
    coordinates[0] = x;
    coordinates[1] = y;
}

void Node::setRoom(bool isRoom)
{
    room = isRoom;
}

void Node::setVisited(int dir)
{
    visited[dir] = true;
}


/**************************************************************************/
/*                           Gets                                         */
/**************************************************************************/


int* Node::getCoord()
{
    return coordinates;
}

int* Node::getDist()
{
    return nodeDist;
}

Node* Node::getN()
{
    return nodePts[0];
}

Node* Node::getS()
{
    return nodePts[1];
}

Node* Node::getE()
{
    return nodePts[2];
}

Node* Node::getW()
{
    return nodePts[3];
}

Node* Node::getPtr(int i)
{
    return nodePts[i];
}

char Node::getNext()
{
    return next;
}

char Node::getPrev()
{
    return prev;
}

int Node::getTravelDist()
{
    return travelDist;
}

bool* Node::getVisited()
{
    return visited;
}

/**************************************************************************/
/*                           Methods                                      */
/**************************************************************************/


int Node::dirtoi(char dir)
{
    switch(dir)
    {
    case 'N':
        return 0;
    case 'S':
        return 1;
    case 'E':
        return 2;
    case 'W':
        return 3;
    default:
        return -1;
    }
}

char Node::itodir(int dir)
{
    switch(dir)
    {
    case 0:
        return 'N';
    case 1:
        return 'S';
    case 2:
        return 'E';
    case 3:
        return 'W';
    default:
        return 'X';
    }
}

char Node::oppDir(char dir)
{
    switch(dir)
    {
    case 'N':
        return 'S';
    case 'S':
        return 'N';
    case 'E':
        return 'W';
    case 'W':
        return 'E';
    default:
        return 'X';
    }
}

/**************************************************************************/
/**************************************************************************/
/*                           Iter Code                                    */
/**************************************************************************/
/**************************************************************************/
